#include "PowerFlowCalculation.h"



gsl_matrix* EqualJacobiMatrix(Node m_Node[],gsl_matrix* m_G,gsl_matrix *m_B,int Node_Num,int GenCount)
{
    
    gsl_matrix* h1=gsl_matrix_alloc(GenCount,Node_Num-1);
    gsl_matrix* h2=gsl_matrix_alloc(GenCount,Node_Num);
    gsl_matrix* h3=gsl_matrix_alloc(Node_Num,Node_Num-1);
    gsl_matrix* h4=gsl_matrix_alloc(Node_Num,Node_Num);
    gsl_matrix* h5=gsl_matrix_alloc(Node_Num-1,Node_Num-1);
    gsl_matrix* h6=gsl_matrix_alloc(Node_Num-1,Node_Num);

    int k=0;
    for(int i=0;i<Node_Num;i++){
        if(m_Node[i].Type == 1){
            for(int j=0;j<Node_Num;j++){
                if(i == j){
                    gsl_matrix_set(h2,k,j,1.0);
                }
            }
            k++;
        }
    }
    
    

    for(int i=0;i<Node_Num-1;i++){
        for(int j=0;j<Node_Num;j++){
            if(i==j){
                gsl_matrix_set(h3,i,i,-m_Node[i].Vol*gsl_matrix_get(m_G,i,i)-m_Node[i].P/m_Node[i].Vol);
            }else{
                double f=m_Node[i].Angle-m_Node[j].Angle;
                gsl_matrix_set(h3,j,i,-m_Node[i].Vol*(gsl_matrix_get(m_G,i,j)*cos(f)+gsl_matrix_get(m_B,i,j)*sin(f)));
            }
        }
    }
    
    for(int i=0;i<Node_Num;i++){
        for(int j=0;j<Node_Num;j++){
            if(i==j){
                gsl_matrix_set(h4,i,i,m_Node[i].Vol*gsl_matrix_get(m_B,i,i)-m_Node[i].Q/m_Node[i].Vol);
            }else{
                double f=m_Node[i].Angle-m_Node[j].Angle;
                gsl_matrix_set(h4,j,i,-m_Node[i].Vol*(gsl_matrix_get(m_G,i,j)*sin(f)-gsl_matrix_get(m_B,i,j)*cos(f)));
            }
        }
    }

    for(int i=0;i<Node_Num-1;i++){
        for(int j=0;j<Node_Num-1;j++){
            if(i==j){
                gsl_matrix_set(h5,i,i,m_Node[i].Vol*m_Node[i].Vol*gsl_matrix_get(m_B,i,i)+m_Node[i].Q);
            }else{
                double f=m_Node[i].Angle-m_Node[j].Angle;
                gsl_matrix_set(h5,j,i,-m_Node[i].Vol*m_Node[j].Vol*(gsl_matrix_get(m_G,i,j)*sin(f)-gsl_matrix_get(m_B,i,j)*cos(f)));
            }
        }
    }

    for(int i=0;i<Node_Num;i++){
        for(int j=0;j<Node_Num-1;j++){
            if(i==j){
                gsl_matrix_set(h6,i,i,m_Node[i].Vol*m_Node[i].Vol*gsl_matrix_get(m_G,i,i)-m_Node[i].P);
            }else{
                double f=m_Node[i].Angle-m_Node[j].Angle;
                gsl_matrix_set(h6,j,i,m_Node[i].Vol*m_Node[j].Vol*(gsl_matrix_get(m_G,i,j)*cos(f)+gsl_matrix_get(m_B,i,j)*sin(f)));
            }
        }
    }

//PRINT_MATRIX(h6)
    gsl_matrix* dst=gsl_matrix_alloc(Node_Num*2-1+GenCount,Node_Num*2-1);
    for(int i=0;i<GenCount;i++){
        for(int j=0;j<Node_Num-1;j++){
            gsl_matrix_set(dst,i,j,gsl_matrix_get(h1,i,j));
        }
    }

    for(int i=0;i<GenCount;i++){
        for(int j=0;j<Node_Num;j++){
            gsl_matrix_set(dst,i,Node_Num-1+j,gsl_matrix_get(h2,i,j));
        }
    }

    for(int i=0;i<Node_Num;i++){
        for(int j=0;j<Node_Num-1;j++){
            gsl_matrix_set(dst,GenCount+i,j,gsl_matrix_get(h3,i,j));
        }
    }
    for(int i=0;i<Node_Num;i++){
        for(int j=0;j<Node_Num;j++){
            gsl_matrix_set(dst,GenCount+i,Node_Num-1+j,gsl_matrix_get(h4,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++){
        for(int j=0;j<Node_Num-1;j++){
            gsl_matrix_set(dst,GenCount+Node_Num+i,j,gsl_matrix_get(h5,i,j));
        }
    }
    for(int i=0;i<Node_Num-1;i++){
        for(int j=0;j<Node_Num;j++){
            gsl_matrix_set(dst,GenCount+Node_Num+i,Node_Num-1+j,gsl_matrix_get(h6,i,j));
        }
    }
    //PRINT_MATRIX(h4)
    gsl_matrix_free(h1);
    gsl_matrix_free(h2);
    gsl_matrix_free(h3);
    gsl_matrix_free(h4);
    gsl_matrix_free(h5);
    gsl_matrix_free(h6);

    return dst;
    
    //gsl_matrix_get
    //gsl_matrix* h1=gsl_matrix_alloc(GenCount,Node_Num-1);

}


